#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char **argv) {
    ros::init(argc, argv, "cre_navigation_node");
    ros::NodeHandle nh;

    // 创建一个MoveBaseClient对象，用于与move_base通信
    MoveBaseClient ac("move_base", true);

    // 等待move_base服务器启动
    while (!ac.waitForServer(ros::Duration(5.0))) {
        ROS_INFO("Waiting for move_base action server to start...");
    }

    // 定义一组导航点数组，可以根据实际需求修改
    double points[][3] = {{0.0,  0.0,  0.0},
                          {1.0,  1.0,  0.0},
                          {-1.0, 1.0,  0.0},
                          {-1.0, -1.0, 0.0},
                          {1.0,  -1.0, 0.0}};

    // 循环发送导航点
    for (int i = 0; i < 5; i++) {
        // 创建一个MoveBaseGoal对象，设置导航目标
        move_base_msgs::MoveBaseGoal goal;
        goal.target_pose.header.frame_id = "map";
        goal.target_pose.header.stamp = ros::Time::now();
        goal.target_pose.pose.position.x = points[i][0];
        goal.target_pose.pose.position.y = points[i][1];
        goal.target_pose.pose.position.z = points[i][2];
        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z = 0.0;
        goal.target_pose.pose.orientation.w = 1.0;

        // 发送导航目标，等待move_base的响应
        ac.sendGoal(goal);
        ac.waitForResult();

        // 判断导航是否成功
        if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
            ROS_INFO("Navigation to point %d succeeded!", i + 1);
        } else {
            ROS_INFO("Navigation to point %d failed!", i + 1);
        }
    }

    return 0;
}
